#include <avr/interrupt.h>
#include "rot_servo.h"
#include "rot_tsetups.h"

volatile unsigned int ppm_vals[CHANNEL_NB]; //servo positions
volatile uint16_t H_COUNTER2; // High Register for Counter TCNT2
volatile int SERVO_POS_CNTS; // Servo position in counts (62~125)

ISR(TIMER1_CAPT_vect){

  static unsigned int lasticr; //icr at last caputre
  static unsigned char cur_ch; //current channel
  unsigned int licr;

  licr=ICR1-lasticr;
  lasticr=ICR1;
  
  if(licr>4500){ //pulse too long, means start of new frame
    cur_ch=0;
  }else if(licr>1000){ //pulse good, take reading, go to next channel
    ppm_vals[cur_ch]=licr;
    if(cur_ch<CHANNEL_NB){
      cur_ch++;
    }
  }else{
    //too short, nothing to see here
  }
}

ISR(TIMER2_OVF_vect) {

  H_COUNTER2++;
  /* If H_COUNTER2=1250, time to reset */
  if (H_COUNTER2 >= 1250){
    PWM_HIGH(SERVO_PORT,SERVO_PIN);
    H_COUNTER2 = 0;
  } 
  /* If is higher than SERVO_POS, go low */
  else if (H_COUNTER2 >= SERVO_POS_CNTS) {
    PWM_LOW(SERVO_PORT,SERVO_PIN);
  } else {
    /* Lower than SERVO_POS, keep high */
  }
  manage_tasks();
  
}

void servo_init(void) {

  int init_pos = 2000;

  SET_SERVO(SERVO_DDR,SERVO_PIN);
  PWM_HIGH(SERVO_PORT,SERVO_PIN);
  MOVE_SERVO_TO(init_pos);

}

int saturate(int pos, int min, int max) {
  
  if (pos>max) {
    pos = max;
  } else if (pos<min) {
    pos = min;
  } else {
  }
  return pos;

}

int pos_to_cnts(int pos) {

  int cnts = (pos/2-1000)/16+62;

  return cnts;

}
